
'''
6.30:滑块下连控卡顿
7.15:简化程序

'''
from ctypes import *
import platform
import threading
import time
import sys
import threading  # 多线程库
import tkinter as tk
from tkinter import ttk
import math
import numpy as np

ZCAN_DEVICE_TYPE = c_uint

INVALID_DEVICE_HANDLE  = 0
INVALID_CHANNEL_HANDLE = 0

'''
 Device Type
'''
ZCAN_USBCANFD_200U    = ZCAN_DEVICE_TYPE(41)
ZCAN_USBCANFD_100U    = ZCAN_DEVICE_TYPE(42)
ZCAN_USBCANFD_MINI    = ZCAN_DEVICE_TYPE(43)


'''
 Interface return status
'''
ZCAN_STATUS_ERR         = 0
ZCAN_STATUS_OK          = 1
ZCAN_STATUS_ONLINE      = 2
ZCAN_STATUS_OFFLINE     = 3
ZCAN_STATUS_UNSUPPORTED = 4

'''
 CAN type
'''
ZCAN_TYPE_CAN    = c_uint(0)
ZCAN_TYPE_CANFD  = c_uint(1)

def input_thread():
   input()

joint_angle_1 = 0
joint_angle_2 = 0
joint_angle_3 = 0
joint_angle_4 = 0
joint_angle_5 = 0
joint_angle_6 = 0
joint_angle_7 = 0
transmit_num = 1
chn_handle = 0

'''
 Device information
'''
class ZCAN_DEVICE_INFO(Structure):
    _fields_ = [("hw_Version", c_ushort),
                ("fw_Version", c_ushort),
                ("dr_Version", c_ushort),
                ("in_Version", c_ushort),
                ("irq_Num", c_ushort),
                ("can_Num", c_ubyte),
                ("str_Serial_Num", c_ubyte * 20),
                ("str_hw_Type", c_ubyte * 40),
                ("reserved", c_ushort * 4)]

    def __str__(self):
        return "Hardware Version:%s\nFirmware Version:%s\nDriver Interface:%s\nInterface Interface:%s\nInterrupt Number:%d\nCAN Number:%d\nSerial:%s\nHardware Type:%s\n" %( \
                self.hw_version, self.fw_version, self.dr_version, self.in_version, self.irq_num, self.can_num, self.serial, self.hw_type)

    def _version(self, version):
        return ("V%02x.%02x" if version // 0xFF >= 9 else "V%d.%02x") % (version // 0xFF, version & 0xFF)

    @property
    def hw_version(self):
        return self._version(self.hw_Version)

    @property
    def fw_version(self):
        return self._version(self.fw_Version)

    @property
    def dr_version(self):
        return self._version(self.dr_Version)

    @property
    def in_version(self):
        return self._version(self.in_Version)

    @property
    def irq_num(self):
        return self.irq_Num

    @property
    def can_num(self):
        return self.can_Num

    @property
    def serial(self):
        serial = ''
        for c in self.str_Serial_Num:
            if c > 0:
               serial += chr(c)
            else:
                break
        return serial

    @property
    def hw_type(self):
        hw_type = ''
        for c in self.str_hw_Type:
            if c > 0:
                hw_type += chr(c)
            else:
                break
        return hw_type

class _ZCAN_CHANNEL_CAN_INIT_CONFIG(Structure):
    _fields_ = [("acc_code", c_uint),
                ("acc_mask", c_uint),
                ("reserved", c_uint),
                ("filter",   c_ubyte),
                ("timing0",  c_ubyte),
                ("timing1",  c_ubyte),
                ("mode",     c_ubyte)]

class _ZCAN_CHANNEL_CANFD_INIT_CONFIG(Structure):
    _fields_ = [("acc_code",     c_uint),
                ("acc_mask",     c_uint),
                ("abit_timing",  c_uint),
                ("dbit_timing",  c_uint),
                ("brp",          c_uint),
                ("filter",       c_ubyte),
                ("mode",         c_ubyte),
                ("pad",          c_ushort),
                ("reserved",     c_uint)]

class _ZCAN_CHANNEL_INIT_CONFIG(Union):
    _fields_ = [("can", _ZCAN_CHANNEL_CAN_INIT_CONFIG), ("canfd", _ZCAN_CHANNEL_CANFD_INIT_CONFIG)]

class ZCAN_CHANNEL_INIT_CONFIG(Structure):
    _fields_ = [("can_type", c_uint),
                ("config", _ZCAN_CHANNEL_INIT_CONFIG)]

class ZCAN_CHANNEL_ERR_INFO(Structure):
    _fields_ = [("error_code", c_uint),
                ("passive_ErrData", c_ubyte * 3),
                ("arLost_ErrData", c_ubyte)]

class ZCAN_CHANNEL_STATUS(Structure):
    _fields_ = [("errInterrupt", c_ubyte),
                ("regMode",      c_ubyte),
                ("regStatus",    c_ubyte),
                ("regALCapture", c_ubyte),
                ("regECCapture", c_ubyte),
                ("regEWLimit",   c_ubyte),
                ("regRECounter", c_ubyte),
                ("regTECounter", c_ubyte),
                ("Reserved",     c_ubyte)]

class ZCAN_CAN_FRAME(Structure):
    _fields_ = [("can_id",  c_uint, 29),
                ("err",     c_uint, 1),
                ("rtr",     c_uint, 1),
                ("eff",     c_uint, 1),
                ("can_dlc", c_ubyte),
                ("__pad",   c_ubyte),
                ("__res0",  c_ubyte),
                ("__res1",  c_ubyte),
                ("data",    c_ubyte * 8)]

class ZCAN_CANFD_FRAME(Structure):
    _fields_ = [("can_id", c_uint, 29),
                ("err",    c_uint, 1),
                ("rtr",    c_uint, 1),
                ("eff",    c_uint, 1),
                ("len",    c_ubyte),
                ("brs",    c_ubyte, 1),
                ("esi",    c_ubyte, 1),
                ("__pad",  c_ubyte, 6),
                ("__res0", c_ubyte),
                ("__res1", c_ubyte),
                ("data",   c_ubyte * 64)]

class ZCAN_Transmit_Data(Structure):
    _fields_ = [("frame", ZCAN_CAN_FRAME), ("transmit_type", c_uint)]

class ZCAN_Receive_Data(Structure):
    _fields_  = [("frame", ZCAN_CAN_FRAME), ("timestamp", c_ulonglong)]

class ZCAN_TransmitFD_Data(Structure):
    _fields_ = [("frame", ZCAN_CANFD_FRAME), ("transmit_type", c_uint)]

class ZCAN_ReceiveFD_Data(Structure):
    _fields_ = [("frame", ZCAN_CANFD_FRAME), ("timestamp", c_ulonglong)]

class ZCAN_AUTO_TRANSMIT_OBJ(Structure):
    _fields_ = [("enable",   c_ushort),
                ("index",    c_ushort),
                ("interval", c_uint),
                ("obj",      ZCAN_Transmit_Data)]

class ZCANFD_AUTO_TRANSMIT_OBJ(Structure):
    _fields_ = [("enable",   c_ushort),
                ("index",    c_ushort),
                ("interval", c_uint),
                ("obj",      ZCAN_TransmitFD_Data)]

class ZCANFD_AUTO_TRANSMIT_OBJ_PARAM(Structure):   #auto_send delay
    _fields_ = [("indix",  c_ushort),
                ("type",   c_ushort),
                ("value",  c_uint)]

class IProperty(Structure):
    _fields_ = [("SetValue", c_void_p),
                ("GetValue", c_void_p),
                ("GetPropertys", c_void_p)]


#定义ZCAN类
class ZCAN(object):
    def __init__(self):
        if platform.system() == "Windows":
            self.__dll = windll.LoadLibrary("./zlgcan.dll")
        else:
            print("No support now!")
        if self.__dll == None:
            print("DLL couldn't be loaded!")

    #定义打开设备函数
    def OpenDevice(self, device_type, device_index, reserved):
        try:
            return self.__dll.ZCAN_OpenDevice(device_type, device_index, reserved)
        except:
            print("Exception on OpenDevice!")
            raise

    #定义关闭设备函数
    def CloseDevice(self, device_handle):
        try:
            return self.__dll.ZCAN_CloseDevice(device_handle)
        except:
            print("Exception on CloseDevice!")
            raise

    #定义获取信息函数
    def GetDeviceInf(self, device_handle):
        try:
            info = ZCAN_DEVICE_INFO()
            ret = self.__dll.ZCAN_GetDeviceInf(device_handle, byref(info))
            return info if ret == ZCAN_STATUS_OK else None
        except:
            print("Exception on ZCAN_GetDeviceInf")
            raise

    #定义设备在线函数
    def DeviceOnLine(self, device_handle):
        try:
            return self.__dll.ZCAN_IsDeviceOnLine(device_handle)
        except:
            print("Exception on ZCAN_ZCAN_IsDeviceOnLine!")
            raise

    #定义CAN初始化函数
    def InitCAN(self, device_handle, can_index, init_config):
        try:
            return self.__dll.ZCAN_InitCAN(device_handle, can_index, byref(init_config))
        except:
            print("Exception on ZCAN_InitCAN!")
            raise

    #定义打开CAN通道函数
    def StartCAN(self, chn_handle):
        try:
            return self.__dll.ZCAN_StartCAN(chn_handle)
        except:
            print("Exception on ZCAN_StartCAN!")
            raise

    #定义复位CAN通道函数
    def ResetCAN(self, chn_handle):
        try:
            return self.__dll.ZCAN_ResetCAN(chn_handle)
        except:
            print("Exception on ZCAN_ResetCAN!")
            raise

    #定义清除缓冲区函数
    def ClearBuffer(self, chn_handle):
        try:
            return self.__dll.ZCAN_ClearBuffer(chn_handle)
        except:
            print("Exception on ZCAN_ClearBuffer!")
            raise

    #定义读取通道错误信息函数
    def ReadChannelErrInfo(self, chn_handle):
        try:
            ErrInfo = ZCAN_CHANNEL_ERR_INFO()
            ret = self.__dll.ZCAN_ReadChannelErrInfo(chn_handle, byref(ErrInfo))
            return ErrInfo if ret == ZCAN_STATUS_OK else None
        except:
            print("Exception on ZCAN_ReadChannelErrInfo!")
            raise

    #定义读取通道状态函数
    def ReadChannelStatus(self, chn_handle):
        try:
            status = ZCAN_CHANNEL_STATUS()
            ret = self.__dll.ZCAN_ReadChannelStatus(chn_handle, byref(status))
            return status if ret == ZCAN_STATUS_OK else None
        except:
            print("Exception on ZCAN_ReadChannelStatus!")
            raise
    #定义接收数据个数
    def GetReceiveNum(self, chn_handle, can_type = ZCAN_TYPE_CAN):
        try:
            return self.__dll.ZCAN_GetReceiveNum(chn_handle, can_type)
        except:
            print("Exception on ZCAN_GetReceiveNum!")
            raise

    #定义数据传输函数
    def Transmit(self, chn_handle, std_msg, len):
        try:
            return self.__dll.ZCAN_Transmit(chn_handle, byref(std_msg), len)
        except:
            print("Exception on ZCAN_Transmit!")
            raise

    #定义接收数据函数
    def Receive(self, chn_handle, rcv_num, wait_time = c_int(-1)):
        try:
            rcv_can_msgs = (ZCAN_Receive_Data * rcv_num)()
            ret = self.__dll.ZCAN_Receive(chn_handle, byref(rcv_can_msgs), rcv_num, wait_time)
            return rcv_can_msgs, ret
        except:
            print("Exception on ZCAN_Receive!")
            raise


    #定义CANFD传输数据函数
    def TransmitFD(self, chn_handle, fd_msg, len):
        try:
            return self.__dll.ZCAN_TransmitFD(chn_handle, byref(fd_msg), len)
        except:
            print("Exception on ZCAN_TransmitFD!")
            raise

    #定义CANFD接收数据函数
    def ReceiveFD(self, chn_handle, rcv_num, wait_time = c_int(-1)):
        try:
            rcv_canfd_msgs = (ZCAN_ReceiveFD_Data * rcv_num)()
            ret = self.__dll.ZCAN_ReceiveFD(chn_handle, byref(rcv_canfd_msgs), rcv_num, wait_time)
            return rcv_canfd_msgs, ret
        except:
            print("Exception on ZCAN_ReceiveFD!")
            raise

    def GetIProperty(self, device_handle):
        try:
            self.__dll.GetIProperty.restype = POINTER(IProperty)
            return self.__dll.GetIProperty(device_handle)
        except:
            print("Exception on ZCAN_GetIProperty!")
            raise

    def SetValue(self, iproperty, path, value):
        try:
            func = CFUNCTYPE(c_uint, c_char_p, c_char_p)(iproperty.contents.SetValue)
            return func(c_char_p(path.encode("utf-8")), c_char_p(value.encode("utf-8")))
        except:
            print("Exception on IProperty SetValue")
            raise

    def SetValue1(self, iproperty, path, value):
        try:
            func = CFUNCTYPE(c_uint, c_char_p, c_char_p)(iproperty.contents.SetValue)
            return func(c_char_p(path.encode("utf-8")), c_void_p(value))
        except:
            print("Exception on IProperty SetValue")
            raise


    def GetValue(self, iproperty, path):
        try:
            func = CFUNCTYPE(c_char_p, c_char_p)(iproperty.contents.GetValue)
            return func(c_char_p(path.encode("utf-8")))
        except:
            print("Exception on IProperty GetValue")
            raise

    def ReleaseIProperty(self, iproperty):
        try:
            return self.__dll.ReleaseIProperty(iproperty)
        except:
            print("Exception on ZCAN_ReleaseIProperty!")
            raise


zcanlib = ZCAN()
testcantype = 0 #0:CAN; 1:canfd
handle = zcanlib.OpenDevice(ZCAN_USBCANFD_MINI,0,0) 

###############################################################################
'''
USBCANFD-MINI Demo
'''
#CANFD启动函数
def canfd_start(device_handle, chn):
    global AutoCAN_B_delay
    global chn_handle
    ip = zcanlib.GetIProperty(device_handle)
    ret = zcanlib.SetValue(ip, str(chn) + "/canfd_standard", "0")
    if ret != ZCAN_STATUS_OK:
        print("Set CH%d CANFD standard failed!" %(chn))
        exit(0)
    ret = zcanlib.SetValue(ip, str(chn) + "/initenal_resistance", "1")
    if ret != ZCAN_STATUS_OK:
        print("Open CH%d resistance failed!" %(chn))
        exit(0)
    ret = zcanlib.SetValue(ip,str(chn)+"/canfd_abit_baud_rate","1000000")  #设置仲裁域波特率
    ret = zcanlib.SetValue(ip,str(chn)+"/canfd_dbit_baud_rate","5000000")   #设置数据域波特率
    if ret != ZCAN_STATUS_OK:
        print("Set CH%d baud failed!" %(chn))
        exit(0)

    ret = zcanlib.SetValue(ip, "0/set_cn","A001")
    if ret == ZCAN_STATUS_OK:
        t = zcanlib.GetValue(ip, "0/get_cn/1")
        print(type(t))
        print(str(t))

###通道配置
    chn_init_cfg = ZCAN_CHANNEL_INIT_CONFIG()
    chn_init_cfg.can_type = ZCAN_TYPE_CANFD
    chn_init_cfg.config.canfd.mode  = 0
    chn_handle = zcanlib.InitCAN(device_handle, chn, chn_init_cfg)
    if chn_handle ==0:
        print("initCAN failed!" %(chn))
        exit(0)

###设置滤波
    ret = zcanlib.SetValue(ip,str(chn)+"/filter_clear","0")
    if ret != ZCAN_STATUS_OK:
        print("Set CH%d  filter_clear failed!" %(chn))
        exit(0)
    ret = zcanlib.SetValue(ip,str(chn)+"/filter_mode","0")    #标准帧滤波
    if ret != ZCAN_STATUS_OK:
        print("Set CH%d  filter_mode failed!" %(chn))
        exit(0)
    ret = zcanlib.SetValue(ip,str(chn)+"/filter_start","0")
    if ret != ZCAN_STATUS_OK:
        print("Set CH%d  filter_start failed!" %(chn))
        exit(0)
    ret = zcanlib.SetValue(ip,str(chn)+"/filter_end","0x7FF")
    if ret != ZCAN_STATUS_OK:
        print("Set CH%d  filter_end failed!" %(chn))
        exit(0)
    ret = zcanlib.SetValue(ip,str(chn)+"/filter_mode","1")    #扩展帧滤波
    if ret != ZCAN_STATUS_OK:
        print("Set CH%d  filter_mode failed!" %(chn))
        exit(0)
    ret = zcanlib.SetValue(ip,str(chn)+"/filter_start","0")
    if ret != ZCAN_STATUS_OK:
        print("Set CH%d  filter_start failed!" %(chn))
        exit(0)
    ret = zcanlib.SetValue(ip,str(chn)+"/filter_end","0x1FFFFFFF")
    if ret != ZCAN_STATUS_OK:
        print("Set CH%d  filter_end failed!" %(chn))
        exit(0)
    ret = zcanlib.SetValue(ip,str(chn)+"/filter_ack","0")
    if ret != ZCAN_STATUS_OK:
        print("Set CH%d  filter_ack failed!" %(chn))
        exit(0)

    ret=zcanlib.StartCAN(chn_handle)
    if ret != ZCAN_STATUS_OK:
        print("startCAN failed!" %(str(chn)))
        exit(0)

### 设置自动发送
    ret = zcanlib.SetValue(ip,str(chn)+"/clear_auto_send","0")
    if ret != ZCAN_STATUS_OK:
        print("Clear CH%d USBCANFD AutoSend failed!" %(chn))
        exit(0)

    path = str(chn)+"/auto_send"
    func = CFUNCTYPE(c_uint, c_char_p, c_void_p)(ip.contents.SetValue)

    path = str(chn)+"/auto_send_param"
    ret = func(c_char_p(path.encode("utf-8")), cast(byref(AutoCAN_B_delay), c_void_p))  #delay 100ms
    '''   
    ret = zcanlib.SetValue(ip,str(chn)+"/apply_auto_send","0")  
    if ret != ZCAN_STATUS_OK:
        print("Apply CH%d USBCANFD AutoSend failed!" %(chn))
        exit(0)
    '''

    zcanlib.ReleaseIProperty(ip)
    return chn_handle


#IAP在线更新标志位（必要操作）
def UpdateFlagBit():
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    transmit_canfd_num = 10
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0  # 0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff = 0  # 0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr = 0  # 0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs = 0  # BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id = 0x01  # 设置CAN-ID
        canfd_msgs[i].frame.len = 3  # 数据长度
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = 0x02
            canfd_msgs[i].frame.data[1] = 0x49
            canfd_msgs[i].frame.data[2] = 0x00
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)

#关闭速度前馈
def CloseSpeedFeedforward():
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    transmit_canfd_num = 10
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0  # 0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff = 0  # 0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr = 0  # 0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs = 0  # BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id = 0x01  # 设置CAN-ID
        canfd_msgs[i].frame.len = 3  # 数据长度
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = 0x02
            canfd_msgs[i].frame.data[1] = 0x39
            canfd_msgs[i].frame.data[2] = 0x00
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)


#获取当前关节状态
def GetJointState(can_id):
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    transmit_canfd_num = 10
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0  # 0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff = 0  # 0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr = 0  # 0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs = 0  # BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id = can_id  # 设置CAN-ID ex.0x605
        canfd_msgs[i].frame.len = 0  # 数据长度
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)
    return ret


#设置位置模式
def SetPositionMode():
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    transmit_canfd_num = 10
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0  # 0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff = 0  # 0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr = 0  # 0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs = 0  # BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id = 0x01  # 设置CAN-ID
        canfd_msgs[i].frame.len = 3  # 数据长度
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = 0x02
            canfd_msgs[i].frame.data[1] = 0x30
            canfd_msgs[i].frame.data[2] = 0x03
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)



#设置电流模式控制
def SetCurrentMode():
    global transmit_num
    global canfd_msgs
    global transmit_canfd_num
    transmit_canfd_num = 10
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0  # 0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff = 0  # 0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr = 0  # 0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs = 0  # BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id = 0x01  # 设置CAN-ID
        canfd_msgs[i].frame.len = 3  # 数据长度
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = 0x02
            canfd_msgs[i].frame.data[1] = 0x30
            canfd_msgs[i].frame.data[2] = 0x01
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)


#设置速度模式控制
def SetSpeedMode():
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    transmit_canfd_num = 10
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0  # 0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff = 0  # 0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr = 0  # 0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs = 0  # BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id = 0x01  # 设置CAN-ID
        canfd_msgs[i].frame.len = 3  # 数据长度
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = 0x02
            canfd_msgs[i].frame.data[1] = 0x30
            canfd_msgs[i].frame.data[2] = 0x02
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)


#模式选择

def ModeSelection(mode):
    if mode == 0:
        SetPositionMode()
        print("位置控制模式")
    if mode == 1:
        SetCurrentMode()
        print("电流控制模式")
    if  mode == 2:
        SetSpeedMode()
        print("速度控制模式")

#清除错误
def ClearErr():
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    transmit_canfd_num = 10
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0  # 0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff = 0  # 0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr = 0  # 0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs = 0  # BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id = 0x01  # 设置CAN-ID
        canfd_msgs[i].frame.len = 3  # 数据长度
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = 0x02
            canfd_msgs[i].frame.data[1] = 0x0f
            canfd_msgs[i].frame.data[2] = 0x01
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)


#设置关节零位
def SetJointZeroPosition():
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    transmit_canfd_num = 10
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0  # 0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff = 0  # 0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr = 0  # 0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs = 0  # BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id = 0x01  # 设置CAN-ID
        canfd_msgs[i].frame.len = 3  # 数据长度
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = 0x02
            canfd_msgs[i].frame.data[1] = 0x0e
            canfd_msgs[i].frame.data[2] = 0x01
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)

#设置上使能
def UpperEnable():
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    transmit_canfd_num = 10
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0  # 0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff = 0  # 0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr = 0  # 0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs = 0  # BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id = 0x01  # 设置CAN-ID
        canfd_msgs[i].frame.len = 3  # 数据长度
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = 0x02
            canfd_msgs[i].frame.data[1] = 0x0a
            canfd_msgs[i].frame.data[2] = 0x01
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)


#设置下使能
def DownEnable():
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    transmit_canfd_num = 10
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0  # 0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff = 0  # 0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr = 0  # 0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs = 0  # BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id = 0x01  # 设置CAN-ID
        canfd_msgs[i].frame.len = 3  # 数据长度
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = 0x02
            canfd_msgs[i].frame.data[1] = 0x0a
            canfd_msgs[i].frame.data[2] = 0x00
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)


#停止运动
def StopMove(can_id):
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0 #0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff     = 0 #0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr     = 0 #0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs     = 0 #BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id  = can_id
        canfd_msgs[i].frame.len     = 4
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = 0x00
            canfd_msgs[i].frame.data[1] = 0x00
            canfd_msgs[i].frame.data[2] = 0x00
            canfd_msgs[i].frame.data[3] = 0x00
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)


def JointMove(can_id, canfd_msgs_data0, canfd_msgs_data1, canfd_msgs_data2, canfd_msgs_data3):
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0 #0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff     = 0 #0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr     = 0 #0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs     = 0 #BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id  = can_id
        canfd_msgs[i].frame.len     = 4
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = canfd_msgs_data0
            canfd_msgs[i].frame.data[1] = canfd_msgs_data1
            canfd_msgs[i].frame.data[2] = canfd_msgs_data2
            canfd_msgs[i].frame.data[3] = canfd_msgs_data3
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)
    # print ("success send count : %d" % ret)



#打开抱闸
def OpenLock():
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    transmit_canfd_num = 10
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()
    for i in range(transmit_num):
        canfd_msgs[i].transmit_type = 0  # 0-正常发送，2-自发自收
        canfd_msgs[i].frame.eff = 0  # 0-标准帧，1-扩展帧
        canfd_msgs[i].frame.rtr = 0  # 0-数据帧，1-远程帧
        canfd_msgs[i].frame.brs = 0  # BRS 加速标志位：0不加速，1加速 要设置为不加速才能成功发送数据
        canfd_msgs[i].frame.can_id = 0x01  # 设置CAN-ID
        canfd_msgs[i].frame.len = 3  # 数据长度
        for j in range(canfd_msgs[i].frame.len):
            canfd_msgs[i].frame.data[0] = 0x02
            canfd_msgs[i].frame.data[1] = 0x30
            canfd_msgs[i].frame.data[2] = 0x03
    ret = zcanlib.TransmitFD(chn_handle, canfd_msgs, transmit_canfd_num)


def hex_concat(int_a, int_b, int_c, int_d):
    # 将应答包返回的4个数据按照协议拼成一个十六进制字符串
    hex_str = hex(int_a)[2:].zfill(2) + hex(int_b)[2:].zfill(2) + hex(int_c)[2:].zfill(2) + hex(int_d)[2:].zfill(2)
    # 将十六进制字符串转换为整数
    num = int(hex_str, 16)

    # 计算符号位
    bit_len = len(hex_str) * 4  # 一个十六进制数占据 4 位二进制数
    sign_bit = (1 << (bit_len - 1)) & num

    if num & sign_bit:
        # 进行补码操作
        num = num - (1 << bit_len)

    return num

# 将10进制目标位置转换成16进制
def dec_hex(dec):
    if dec >= 0:
        return hex(dec)
    else:
        # 取绝对值
        abs_dec = abs(dec)
        # 获取所需位数（32位）
        bit_len = 32
        # 计算补码
        twos_complement = (1 << bit_len) - abs_dec
        # 转换为十六进制
        hex_str = hex(twos_complement)
    return hex_str

def joint_angle_get(motor_id):
    global chn_handle
    real_current_joint = 0
    rcv_canfd_msgs = []
    transmit_canfd_num = 10
    joint_state_can_id = eval('0x60' + str(motor_id))
    joint_state_can_id_ans = joint_state_can_id + 256  # 如果关节id为7，关节角度查询id = 0x607，查询应答id = 0x707
    for i in range(transmit_canfd_num):
        flag = False
        joint_state = GetJointState(joint_state_can_id)
        rcv_canfd_num = zcanlib.GetReceiveNum(chn_handle, ZCAN_TYPE_CANFD)
        if rcv_canfd_num:
            # print("Receive CANFD message number:%d" % rcv_canfd_num)
            rcv_canfd_msgs, rcv_canfd_num = zcanlib.ReceiveFD(chn_handle, rcv_canfd_num, 1000)
            for j in range(rcv_canfd_num):
                if rcv_canfd_msgs[j].frame.can_id == joint_state_can_id_ans:
                # if rcv_canfd_msgs[j].frame.can_id == 1281: #0x507
                    rcv_canfd_msgs_temp = rcv_canfd_msgs[j]
                    # print("[%d]:timestamp:%d,type:canfd, id:%s, len:%d, eff:%d, rtr:%d, esi:%d, brs: %d, data:%s" % (
                    #     j, rcv_canfd_msgs[j].timestamp, hex(rcv_canfd_msgs[j].frame.can_id), rcv_canfd_msgs[j].frame.len,
                    #     rcv_canfd_msgs[j].frame.eff, rcv_canfd_msgs[j].frame.rtr,
                    #     rcv_canfd_msgs[j].frame.esi, rcv_canfd_msgs[j].frame.brs,
                    #     ''.join(hex(rcv_canfd_msgs[j].frame.data[k]) + ' ' for k in range(rcv_canfd_msgs[j].frame.len))))
                    current_joint_state = hex_concat(rcv_canfd_msgs_temp.frame.data[11],
                                rcv_canfd_msgs_temp.frame.data[10],
                                rcv_canfd_msgs_temp.frame.data[9],
                                rcv_canfd_msgs_temp.frame.data[8])
                    real_current_joint = float(current_joint_state)/10000
                    # print("current state: %.4f" % real_current_joint)
                    # break
                # else:
                #     print('[%d]Get joint state data failed! Retrieving' % j)
    return real_current_joint

def setPosition(joint_move_can_id, joint_state_can_id, target_position):
    #读取当前 joint_state
    global chn_handle
    global transmit_canfd_num
    rcv_canfd_msgs = []
    timeout = 0
    rcv_canfd_num = 0
    timestamp_previous = 0
    transmit_canfd_num = 10
    joint_state_can_id_ans = joint_state_can_id + 256  # 如果关节id为7，关节角度查询id = 0x607，查询应答id = 0x707
    previous_time = time.perf_counter()
    #获取一次joint state 作为开始
    for i in range(transmit_canfd_num):
        flag = False
        joint_state = GetJointState(joint_state_can_id)
        print("joint_state" + str(joint_state))
        rcv_canfd_num = zcanlib.GetReceiveNum(chn_handle, ZCAN_TYPE_CANFD)
        # print("rcv_canfd_num:" + str(rcv_canfd_num))
        if rcv_canfd_num:
            # print("Receive CANFD message number:%d" % rcv_canfd_num)
            rcv_canfd_msgs, rcv_canfd_num = zcanlib.ReceiveFD(chn_handle, rcv_canfd_num, 1000)
            for j in range(rcv_canfd_num):
                if rcv_canfd_msgs[j].frame.can_id == joint_state_can_id_ans:
                    rcv_canfd_msgs_temp = rcv_canfd_msgs[j]
                    # print("[%d]:timestamp:%d,type:canfd, id:%s, len:%d, eff:%d, rtr:%d, esi:%d, brs: %d, data:%s" % (
                    #     j, rcv_canfd_msgs[j].timestamp, hex(rcv_canfd_msgs[j].frame.can_id), rcv_canfd_msgs[j].frame.len,
                    #     rcv_canfd_msgs[j].frame.eff, rcv_canfd_msgs[j].frame.rtr,
                    #     rcv_canfd_msgs[j].frame.esi, rcv_canfd_msgs[j].frame.brs,
                    #     ''.join(hex(rcv_canfd_msgs[j].frame.data[k]) + ' ' for k in range(rcv_canfd_msgs[j].frame.len))))
                    flag = True
                    break                 
                # else:
                #     print('Get joint state data failed! Retrieving')
        if flag:
            #按照应答包协议从后往前拼接（放大10000倍）
            current_joint_state = hex_concat(rcv_canfd_msgs_temp.frame.data[11],
                                            rcv_canfd_msgs_temp.frame.data[10],
                                            rcv_canfd_msgs_temp.frame.data[9],
                                            rcv_canfd_msgs_temp.frame.data[8])
            break

    if flag:
        for i in range (int(abs(target_position - current_joint_state)/10000)):
                
                #设置目标位置
                if target_position - current_joint_state > 10000:
                    target_joint_state = current_joint_state + (i+1)*10000
                elif target_position - current_joint_state < -10000:
                    target_joint_state = current_joint_state - (i+1)*10000
                #将10进制目标位置转换成16进制，如果不够8位自动补0占位
                hex_target_joint_state = dec_hex(target_joint_state)[2:].zfill(8)
                print("target joint state = %.3f degree" % (float(target_joint_state)/10000))
                #填入对应的十六进制数据
                canfd_msgs_data0 = int(hex_target_joint_state[6:8],16)
                canfd_msgs_data1  = int(hex_target_joint_state[4:6],16)
                canfd_msgs_data2   = int(hex_target_joint_state[2:4],16)
                canfd_msgs_data3   = int(hex_target_joint_state[0:2],16)

                JointMove(joint_move_can_id,canfd_msgs_data0,canfd_msgs_data1,canfd_msgs_data2,canfd_msgs_data3)     #关节运动
                time.sleep(0.001)

        #将10进制目标位置转换成16进制，如果不够8位自动补0占位
        hex_target_joint_state = dec_hex(target_position)[2:].zfill(8)
        #填入对应的十六进制数据
        canfd_msgs_data0 = int(hex_target_joint_state[6:8],16)
        canfd_msgs_data1  = int(hex_target_joint_state[4:6],16)
        canfd_msgs_data2   = int(hex_target_joint_state[2:4],16)
        canfd_msgs_data3   = int(hex_target_joint_state[0:2],16)

        JointMove(joint_move_can_id,canfd_msgs_data0,canfd_msgs_data1,canfd_msgs_data2,canfd_msgs_data3)     #关节运动
    else:
        print('Get joint state data failed! End Joint Move')
        return 0
    #返回结果不准，需要等待
    # time.sleep(3)
    #获取一次joint state 作为结束
    for i in range(transmit_canfd_num):
        flag = False
        joint_state = GetJointState(joint_state_can_id)
        rcv_canfd_num = zcanlib.GetReceiveNum(chn_handle, ZCAN_TYPE_CANFD)
        if rcv_canfd_num:
            # print("Receive CANFD message number:%d" % rcv_canfd_num)
            rcv_canfd_msgs, rcv_canfd_num = zcanlib.ReceiveFD(chn_handle, rcv_canfd_num, 1000)
            for j in range(rcv_canfd_num):
                if rcv_canfd_msgs[j].frame.can_id == joint_state_can_id_ans:
                # if rcv_canfd_msgs[j].frame.can_id == 1281: #0x507
                    rcv_canfd_msgs_temp = rcv_canfd_msgs[j]
                    # print("[%d]:timestamp:%d,type:canfd, id:%s, len:%d, eff:%d, rtr:%d, esi:%d, brs: %d, data:%s" % (
                    #     j, rcv_canfd_msgs[j].timestamp, hex(rcv_canfd_msgs[j].frame.can_id), rcv_canfd_msgs[j].frame.len,
                    #     rcv_canfd_msgs[j].frame.eff, rcv_canfd_msgs[j].frame.rtr,
                    #     rcv_canfd_msgs[j].frame.esi, rcv_canfd_msgs[j].frame.brs,
                    #     ''.join(hex(rcv_canfd_msgs[j].frame.data[k]) + ' ' for k in range(rcv_canfd_msgs[j].frame.len))))
                    current_joint_state = hex_concat(rcv_canfd_msgs_temp.frame.data[11],
                                rcv_canfd_msgs_temp.frame.data[10],
                                rcv_canfd_msgs_temp.frame.data[9],
                                rcv_canfd_msgs_temp.frame.data[8])
                    # print("current state: %.4f" % (float(current_joint_state)/10000))
                    # break
                # else:
                #     print('[%d]Get joint state data failed! Retrieving' % j)

    if abs(target_position - current_joint_state) < 1000:
        print("target position has been reached")
        return 1
    else:
        # print("set position fail. target position: %.4f current position: %.4f" % (float(target_position)/10000, float(current_joint_state)/10000))
        return 0
    
def Motor_control(motor_id, angle):
    #填入关节运动模式的CAN-ID，   关节位置伺服运动CAN-ID:0x201  关节速度伺服运动CAN-ID:0x301   关节电流伺服运动CAN-ID：0x401，如果关节id为7则为0x207
    move_can_id = eval('0x20' + str(motor_id))
    state_can_id = eval('0x60' + str(motor_id))
    start_time = time.time()
    # UpperEnable()
    setPosition(move_can_id, state_can_id, math.floor(angle * 10000))  # 10000对应1°
    # DownEnable()
    
    end_time=time.time()
    execution_time = end_time - start_time  

    print(f"Function execution took {execution_time * 1000:.6f} milliseconds")

def close_can():
    #Close CAN
    global chn_handle
    ret=zcanlib.ResetCAN(chn_handle)
    if ret==1:
        print("ResetCAN success! ")
    #Close Device
    ret=zcanlib.CloseDevice(handle)
    if ret==1:
        print("CloseDevice success! ")
    print("Finish!")

def hardware_control():
    global AutoCAN_B_delay
    global transmit_num
    global chn_handle
    global canfd_msgs
    global transmit_canfd_num
    if handle == INVALID_DEVICE_HANDLE:
        print("Open CANFD Device failed!")
        exit(0)
    print("device handle:%d." %(handle))

    info = zcanlib.GetDeviceInf(handle)
    print("Device Information:\n%s" %(info))

    #设置自动发送对象

    AutoCAN_A    =  ZCAN_AUTO_TRANSMIT_OBJ()
    AutoCAN_B    =  ZCAN_AUTO_TRANSMIT_OBJ()
    AutoCAN_A.enable                    = 1  #enable
    AutoCAN_A.index                     = 0
    AutoCAN_A.interval                  = 200  #ms
    AutoCAN_A.obj.frame.can_id          = 0x100
    AutoCAN_A.obj.transmit_type         = 0
    AutoCAN_A.obj.frame.eff             = 0
    AutoCAN_A.obj.frame.rtr             = 0
    AutoCAN_A.obj.frame.can_dlc         = 8
    for j in range(AutoCAN_A.obj.frame.can_dlc):
        AutoCAN_A.obj.frame.data[j] = j

    AutoCAN_B.enable                    = 1  #enable
    AutoCAN_B.index                     = 1
    AutoCAN_B.interval                  = 200  #ms
    AutoCAN_B.obj.frame.can_id          = 0x300
    AutoCAN_B.obj.transmit_type         = 0
    AutoCAN_B.obj.frame.eff             = 0
    AutoCAN_B.obj.frame.rtr             = 0
    AutoCAN_B.obj.frame.can_dlc         = 8
    for j in range(AutoCAN_B.obj.frame.can_dlc):
        AutoCAN_B.obj.frame.data[j] = j

    AutoCAN_B_delay=ZCANFD_AUTO_TRANSMIT_OBJ_PARAM()
    AutoCAN_B_delay.index = AutoCAN_B.index
    AutoCAN_B_delay.type  = 1
    AutoCAN_B_delay.value = 100

    #开始CAN通讯

    chn_handle = canfd_start(handle, 0)
    print("channel handle:%d." %(chn_handle))

    #发送CAN消息

    transmit_num = 1
    msgs = (ZCAN_Transmit_Data * transmit_num)()
    for i in range(transmit_num):
        msgs[i].transmit_type = 0 #0-正常发送，2-自发自收
        msgs[i].frame.eff     = 0 #0-标准帧，1-扩展帧
        msgs[i].frame.rtr     = 0 #0-数据帧，1-远程帧
        msgs[i].frame.can_id  = i
        msgs[i].frame.can_dlc = 8
        for j in range(msgs[i].frame.can_dlc):
            msgs[i].frame.data[j] = j
    print('data',msgs[i].frame.data[j])
    ret = zcanlib.Transmit(chn_handle, msgs, transmit_num)
    print("Tranmit Num: %d." % ret)

    transmit_canfd_num = 1
    canfd_msgs = (ZCAN_TransmitFD_Data * transmit_canfd_num)()

    UpdateFlagBit()         #IAP在线更新标志位


    OpenLock()      #打开抱闸
    time.sleep(1)

    CloseSpeedFeedforward()     #关闭速度前馈

    # time.sleep(2)

    # ModeSelection(0)        # 位置模式：0     电流模式：1      速度模式：2
    # Motor_control(7, 50)
    
    # time.sleep(2)
    # ModeSelection(0)
    # Motor_control(7, 20)

    while True:
        global joint_angle_1, joint_angle_2, joint_angle_3, joint_angle_4, joint_angle_5, joint_angle_6, joint_angle_7
        joint_angle_1 = joint_angle_get(1)
        joint_angle_2 = joint_angle_get(2)
        joint_angle_3 = joint_angle_get(3)
        joint_angle_4 = joint_angle_get(4)
        joint_angle_5 = joint_angle_get(5)
        joint_angle_6 = joint_angle_get(6)
        joint_angle_7 = joint_angle_get(7)

        motor_value_get_entry_1.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black') 
        motor_value_get_entry_1.delete(0, tk.END)
        motor_value_get_entry_1.insert(0, str(joint_angle_1))
        motor_value_get_entry_1.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black') 
        
        motor_value_get_entry_2.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black') 
        motor_value_get_entry_2.delete(0, tk.END)
        motor_value_get_entry_2.insert(0, str(joint_angle_2))
        motor_value_get_entry_2.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black') 
        
        motor_value_get_entry_3.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black') 
        motor_value_get_entry_3.delete(0, tk.END)
        motor_value_get_entry_3.insert(0, str(joint_angle_3))
        motor_value_get_entry_3.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black') 
        
        motor_value_get_entry_4.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black') 
        motor_value_get_entry_4.delete(0, tk.END)
        motor_value_get_entry_4.insert(0, str(joint_angle_4))
        motor_value_get_entry_4.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black') 
        
        motor_value_get_entry_5.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black') 
        motor_value_get_entry_5.delete(0, tk.END)
        motor_value_get_entry_5.insert(0, str(joint_angle_5))
        motor_value_get_entry_5.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black') 
        
        motor_value_get_entry_6.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black') 
        motor_value_get_entry_6.delete(0, tk.END)
        motor_value_get_entry_6.insert(0, str(joint_angle_6))
        motor_value_get_entry_6.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black') 
        
        motor_value_get_entry_7.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black') 
        motor_value_get_entry_7.delete(0, tk.END)
        motor_value_get_entry_7.insert(0, str(joint_angle_7))
        motor_value_get_entry_7.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black') 

# 控制函数
def control_joint(joint_id, angle):
    print(f"执行控制：关节{joint_id} -> {angle:.5f}°")
    ModeSelection(0)        # 位置模式：0     电流模式：1      速度模式：2
    Motor_control(joint_id, angle)

 #滑块事件回调
def entry1_get():
    print('motor1_set:', motor_value_entry_1.get())
    joint1_set = eval(motor_value_entry_1.get())
    joint1_set_limit = np.clip(joint1_set, -90, 180)
    control_joint(1, joint1_set_limit)

def entry2_get():
    print('motor2_set:', motor_value_entry_2.get())
    joint2_set = eval(motor_value_entry_2.get())
    joint2_set_limit = np.clip(joint2_set, -90, 0)
    control_joint(2, joint2_set_limit)

def entry3_get():
    print('motor3_set:', motor_value_entry_3.get())
    joint3_set = eval(motor_value_entry_3.get())
    joint3_set_limit = np.clip(joint3_set, -120, 120)
    control_joint(3, joint3_set_limit)

def entry4_get():
    print('motor4_set:', motor_value_entry_4.get())
    joint4_set = eval(motor_value_entry_4.get())
    joint4_set_limit = np.clip(joint4_set, 0, 135)
    control_joint(4, joint4_set_limit)

def entry5_get():
    print('motor5_set:', motor_value_entry_5.get())
    joint5_set = eval(motor_value_entry_5.get())
    joint5_set_limit = np.clip(joint5_set, -90, 90)
    control_joint(5, joint5_set_limit)

def entry6_get():
    print('motor6_set:', motor_value_entry_6.get())
    joint6_set = eval(motor_value_entry_6.get())
    joint6_set_limit = np.clip(joint6_set, -90, 90)
    control_joint(6, joint6_set_limit)

def entry7_get():
    print('motor7_set:', motor_value_entry_7.get())
    joint7_set = eval(motor_value_entry_7.get())
    joint7_set_limit = np.clip(joint7_set, -120, 120)
    control_joint(7, joint7_set_limit)

# 滑块事件回调
#def on_slider_change(joint_id, var, label_var):
    #angle = var.get()
    #label_var.set(f"{angle:.2f}")
    #control_joint(joint_id, angle)
# 创建 GUI
root = tk.Tk()
root.title("Robot Arm Real-Time Control")
root.geometry("800x450")

for i in range(7):
    #第一列
    tk.Label(root, text=f"关节{i+1}角度:").grid(row=i, column=0, padx=10, pady=5, sticky='e')

    #第二列
    var = tk.DoubleVar()
    label_var = tk.StringVar()
    slider = ttk.Scale(root, from_=-180, to=180, orient='horizontal', length=300,
                       variable=var, command=lambda val, jid=i, v=var, lv=label_var: on_slider_change(jid+1, v, lv), state = 'enable')
    slider.grid(row=i, column=1, padx=10, pady=5)

    #第三列
    var.set(0.0)
    label_var.set(f"{var.get():.5f}")
    tk.Label(root, textvariable=label_var, width=7).grid(row=i, column=2)

#第四列
motor_value_1 = tk.StringVar()
motor_value_entry_1 = tk.Entry(root, width = 12,textvariable = motor_value_1)
motor_value_entry_1.insert(0, '0')
motor_value_entry_1.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_entry_1.grid(row=0, column=3, padx=10, pady=5)

motor_value_2 = tk.StringVar()
motor_value_entry_2 = tk.Entry(root, width = 12,textvariable = motor_value_2)
motor_value_entry_2.insert(0, '0')
motor_value_entry_2.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_entry_2.grid(row=1, column=3, padx=10, pady=5)

motor_value_3 = tk.StringVar()
motor_value_entry_3 = tk.Entry(root, width = 12,textvariable = motor_value_3)
motor_value_entry_3.insert(0, '0')
motor_value_entry_3.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_entry_3.grid(row=2, column=3, padx=10, pady=5)

motor_value_4 = tk.StringVar()
motor_value_entry_4 = tk.Entry(root, width = 12,textvariable = motor_value_4)
motor_value_entry_4.insert(0, '0')
motor_value_entry_4.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_entry_4.grid(row=3, column=3, padx=10, pady=5)

motor_value_5 = tk.StringVar()
motor_value_entry_5 = tk.Entry(root, width = 12,textvariable = motor_value_5)
motor_value_entry_5.insert(0, '0')
motor_value_entry_5.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_entry_5.grid(row=4, column=3, padx=10, pady=5)

motor_value_6 = tk.StringVar()
motor_value_entry_6 = tk.Entry(root, width = 12,textvariable = motor_value_6)
motor_value_entry_6.insert(0, '0')
motor_value_entry_6.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_entry_6.grid(row=5, column=3, padx=10, pady=5)

motor_value_7 = tk.StringVar()
motor_value_entry_7 = tk.Entry(root, width = 12,textvariable = motor_value_7)
motor_value_entry_7.insert(0, '0')
motor_value_entry_7.configure(state='normal', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_entry_7.grid(row=6, column=3, padx=10, pady=5)

#第五列
tk.Button( root,
        text = "设置参数", # 按键名称
        activebackground="white", # 按键按下时的背景色
        activeforeground="black",# 按键按下时的字体背景色
        bg="white", # 按键背景色
        fg="black", # 按键前景色
        command=entry1_get, #按下时执行的函数命令
        ).grid(row=0, column=4, padx=10, pady=5)

tk.Button( root,
        text = "设置参数", # 按键名称
        activebackground="white", # 按键按下时的背景色
        activeforeground="black",# 按键按下时的字体背景色
        bg="white", # 按键背景色
        fg="black", # 按键前景色
        command=entry2_get, #按下时执行的函数命令
        ).grid(row=1, column=4, padx=10, pady=5)

tk.Button( root,
        text = "设置参数", # 按键名称
        activebackground="white", # 按键按下时的背景色
        activeforeground="black",# 按键按下时的字体背景色
        bg="white", # 按键背景色
        fg="black", # 按键前景色
        command=entry3_get, #按下时执行的函数命令
        ).grid(row=2, column=4, padx=10, pady=5)

tk.Button( root,
        text = "设置参数", # 按键名称
        activebackground="white", # 按键按下时的背景色
        activeforeground="black",# 按键按下时的字体背景色
        bg="white", # 按键背景色
        fg="black", # 按键前景色
        command=entry4_get, #按下时执行的函数命令
        ).grid(row=3, column=4, padx=10, pady=5)

tk.Button( root,
        text = "设置参数", # 按键名称
        activebackground="white", # 按键按下时的背景色
        activeforeground="black",# 按键按下时的字体背景色
        bg="white", # 按键背景色
        fg="black", # 按键前景色
        command=entry5_get, #按下时执行的函数命令
        ).grid(row=4, column=4, padx=10, pady=5)

tk.Button( root,
        text = "设置参数", # 按键名称
        activebackground="white", # 按键按下时的背景色
        activeforeground="black",# 按键按下时的字体背景色
        bg="white", # 按键背景色
        fg="black", # 按键前景色
        command=entry6_get, #按下时执行的函数命令
        ).grid(row=5, column=4, padx=10, pady=5)

tk.Button( root,
        text = "设置参数", # 按键名称
        activebackground="white", # 按键按下时的背景色
        activeforeground="black",# 按键按下时的字体背景色
        bg="white", # 按键背景色
        fg="black", # 按键前景色
        command=entry7_get, #按下时执行的函数命令
        ).grid(row=6, column=4, padx=10, pady=5)

#第六列
motor_value_get_1 = tk.StringVar()
motor_value_get_entry_1 = tk.Entry(root, width = 12,textvariable = motor_value_get_1)
motor_value_get_entry_1.insert(0, '0')
motor_value_get_entry_1.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_get_entry_1.grid(row=0, column=5, padx=10, pady=5)

motor_value_get_2 = tk.StringVar()
motor_value_get_entry_2 = tk.Entry(root, width = 12,textvariable = motor_value_get_2)
motor_value_get_entry_2.insert(0, '0')
motor_value_get_entry_2.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_get_entry_2.grid(row=1, column=5, padx=10, pady=5)

motor_value_get_3 = tk.StringVar()
motor_value_get_entry_3 = tk.Entry(root, width = 12,textvariable = motor_value_get_3)
motor_value_get_entry_3.insert(0, '0')
motor_value_get_entry_3.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_get_entry_3.grid(row=2, column=5, padx=10, pady=5)

motor_value_get_4 = tk.StringVar()
motor_value_get_entry_4 = tk.Entry(root, width = 12,textvariable = motor_value_get_4)
motor_value_get_entry_4.insert(0, '0')
motor_value_get_entry_4.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_get_entry_4.grid(row=3, column=5, padx=10, pady=5)

motor_value_get_5 = tk.StringVar()
motor_value_get_entry_5 = tk.Entry(root, width = 12,textvariable = motor_value_get_5)
motor_value_get_entry_5.insert(0, '0')
motor_value_get_entry_5.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_get_entry_5.grid(row=4, column=5, padx=10, pady=5)

motor_value_get_6 = tk.StringVar()
motor_value_get_entry_6 = tk.Entry(root, width = 12,textvariable = motor_value_get_6)
motor_value_get_entry_6.insert(0, '0')
motor_value_get_entry_6.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_get_entry_6.grid(row=5, column=5, padx=10, pady=5)

motor_value_get_7 = tk.StringVar()
motor_value_get_entry_7 = tk.Entry(root, width = 12,textvariable = motor_value_get_7)
motor_value_get_entry_7.insert(0, joint_angle_7)
motor_value_get_entry_7.configure(state='disable', disabledbackground = 'white',
                        disabledforeground = 'black')
motor_value_get_entry_7.grid(row=6, column=5, padx=10, pady=5)

# 获取当前关节角度（从 GUI 的 motor_value_get_X 读取）
def get_joint_angle(joint_id):
    try:
        return float(globals()[f"motor_value_get_{joint_id}"].get())
    except:
        print(f"无法读取关节{joint_id}角度，默认返回0")
        return 0.0

# 添加总控按钮
def activate_all_settings():
    # 依次执行所有设置参数按钮的功能
    entry1_get()
    entry2_get()
    entry3_get()
    entry4_get()
    entry5_get()
    entry6_get()
    entry7_get()
    print("所有关节参数已同步设置")

# 绑定回车键到 activate_all_settings
#root.bind("<Return>", lambda event: activate_all_settings())

# 创建按钮
tk.Button(root,
          text="一键启动",
          command=activate_all_settings,
          activebackground="white",
          activeforeground="black",
          bg="white",
          fg="black").grid(row=7, column=4, padx=10, pady=5, sticky="ew")

def move_all_joints_two_steps_custom():
    # 每个关节的第一次和第二次增量（可自由调整）
    first_deltas = [20, -5, 0, 60, 0, 30, 0]  # 第一次各关节角度变化
    second_deltas = [40, 0, 0, -20, 0, -25, 0]  # 第二次各关节角度变化
    delay_seconds = 3  # 两次动作间延迟（秒）

    def run():
        # 第一次运动
        for joint_id in range(1, 8):
            current_angle = get_joint_angle(joint_id)
            #xianwei
            control_joint(joint_id, current_angle + first_deltas[joint_id - 1])
        print(f"所有关节第一次运动完成：{first_deltas}")

        # 延迟
        time.sleep(delay_seconds)

        # 第二次运动
        for joint_id in range(1, 8):
            current_angle = get_joint_angle(joint_id)
            control_joint(joint_id, current_angle + second_deltas[joint_id - 1])
        print(f"所有关节第二次运动完成：{second_deltas}")

    threading.Thread(target=run, daemon=True).start()

tk.Button(root,
          text="各关节两段自定义运动",
          command=move_all_joints_two_steps_custom,
          bg="white", fg="black").grid(row=10, column=4, padx=10, pady=5, sticky="ew")

root.bind("<Return>", lambda event: move_all_joints_two_steps_custom())


def robot_arm_GUI():
    root.mainloop()


if __name__ == "__main__":
    hardware_thread = threading.Thread(target=lambda: hardware_control())
    hardware_thread.Daemon = True
    hardware_thread.start()

    robot_arm_GUI()
